#include "PathMap.hpp"

#include <QPoint>
#include <QDebug>

#include <cmath>
#include <assert.h>
#include "../core/constantes.h"

PathMap::PathMap(){
    mIsFlying = false;
}

void PathMap::NodeToXY( void* node, int* x, int* y ){
    int index = (int)node;
    *y = index / mMapWidth;
    *x = index - *y * mMapWidth;
}

float PathMap::LeastCostEstimate( void* nodeStart, void* nodeEnd ){
    int xStart, yStart, xEnd, yEnd;
    NodeToXY( nodeStart, &xStart, &yStart );
    NodeToXY( nodeEnd, &xEnd, &yEnd );

    int dx = xStart - xEnd;
    int dy = yStart - yEnd;
    return (float) sqrt( (double)(dx*dx) + (double)(dy*dy) );
}


void PathMap::AdjacentCost( void* node, std::vector< micropather::StateCost > *neighbors ){
    int x, y;
    const int dx[8] = { 1, 0, -1, 0};
    const int dy[8] = { 0, 1, 0,-1};
    const float cost[8] = { 1.0f, 1.0f, 1.0f, 1.0f};

    NodeToXY( node, &x, &y );

    for( int i=0; i<4; ++i ) {
        int nx = x + dx[i];
        int ny = y + dy[i];

        int pass = passable( nx, ny);
        if ( (pass != c_rift && pass != c_block && !mIsFlying) || mIsFlying ) {
            if ( pass == c_desert || (mIsFlying && (pass == c_rift || pass == c_block))) {
                // Normal floor
                micropather::StateCost nodeCost = { XYToNode( nx, ny ), cost[i] };
                neighbors->push_back( nodeCost );
            }
            else if(pass == c_noise || pass == c_smoke){
                micropather::StateCost nodeCost = { XYToNode( nx, ny ), cost[i] + 4 };
                neighbors->push_back( nodeCost );
            }
            else {
                // Floor to avoid
                micropather::StateCost nodeCost = { XYToNode( nx, ny ), cost[i] + 15 };
                neighbors->push_back( nodeCost );
            }
        }
    }
}

int PathMap::passable(int nx, int ny){
    if (nx >= 0 && nx < mMapWidth
                && ny >= 0 && ny < mMapHeight)
        return mMap.at(ny*mMapWidth + nx);
    else
        return 0;
}

void* PathMap::XYToNode( int x, int y ){
    return (void*) ( y*mMapWidth + x );
}

void PathMap::fillMap(QVector<int> map, int width, int height){
    if(map.size() != width * height){
        Q_ASSERT(map.size() != width * height);
        return;
    }

    mMap = map;
    mMapWidth = width;
    mMapHeight = height;
}
